#ifndef __KALMAN_H
#define __KALMAN_H

#include "DriversStruct.h"

#define RAD_TO_DEG 57.295779513082320876798154814105

typedef struct
{
  double Roll;  // X 轴的卡尔曼滤波计算角度
  double Pitch; // Y 轴的卡尔曼滤波计算角度
  double Yaw;   // Z 轴的卡尔曼滤波计算角度
  float yaw_error;
  float yaw_sumerror;
  float yaw_last; 
} BMI088_t;

typedef struct
{
  double Q_angle;   // 角度过程噪声协方差
  double Q_bias;    // 偏置过程噪声协方差
  double R_measure; // 测量噪声协方差
  double angle;     // 估计角度
  double bias;      // 估计偏置
  double P[2][2];   // 协方差矩阵
} Kalman_t;

extern Kalman_t KalmanX;
extern Kalman_t KalmanY;
extern Kalman_t KalmanZ;

void initialize_BMI088_and_kalman(Kalman_t *KalmanX, Kalman_t *KalmanY, Kalman_t *KalmanZ);
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);
void BMI088_Kalman_Filter(Kalman_t *KalmanX, Kalman_t *KalmanY, Kalman_t *KalmanZ, BMI088_ReceiveDataTypedef *BMI088);

#endif
